//
// Created by PulsarV on 19-2-26.
//

#ifndef USE_DSP_DEVICE

#include <rc_move/rcmove.h>
#include <rc_gpio/c_gpio.h>
#include <rc_gpio/soft_pwm.h>
#include <rc_log/slog.hpp>

#include <mraa/common.hpp>
#include <mraa/gpio.hpp>
#include <mraa/pwm.hpp>

namespace rccore {
    namespace move {
        void RobotCarMove::convert_speed(float speed_v_m_s, const std::shared_ptr<mraa::Pwm> &pwm_port) {
            //角速度=赫兹x细分倍数x200 单位rad/s
            //赫兹=角速度*200/细分倍数
            //赫兹=线速度/车轮半径*200/细分倍数
            //线速度=角速度x车轮半径
            //角速度=线速度/车轮半径
            float hz = speed_v_m_s / (19.0f / 1000.0f) * 200;
            float freq = (1 / hz) * 1000000;
            float width = freq / 2;
            slog::warn << "Speed:" << speed_v_m_s << " HZ:" << (int) hz << " Width:" << (int) width << " Freq:"
                       << (int) freq << slog::endl;

            pwm_port->period_us((int) freq);
            pwm_port->pulsewidth_us((int) width);
        }

        void RobotCarMove::wheel_1_backward(float speed_v_m_s) {
            if (pcontext->pconfig->pconfigInfo->USE_MRAA) {
                slog::warn << __FUNCTION__ << ": " << speed_v_m_s << slog::endl;
                convert_speed(speed_v_m_s, _pwm_1);
                set_value(_gpio_1, 0);
            } else {
                RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_1);
                RC::GPIO::pwm_set_frequency(pcontext->pconfig->pconfigInfo->PWM_1, speed_v_m_s);
                RC::GPIO::digitalWrite(pcontext->pconfig->pconfigInfo->GPIO_1, RC_GPIO_HIGH);
                RC::GPIO::pwm_start(pcontext->pconfig->pconfigInfo->PWM_1);
            }
        }

        void RobotCarMove::wheel_1_forward(float speed_v_m_s) {
            if (pcontext->pconfig->pconfigInfo->USE_MRAA) {
                std::cout << __FUNCTION__ << "wheel_1_forward:" << speed_v_m_s << std::endl;
                convert_speed(speed_v_m_s, _pwm_1);
                set_value(_gpio_1, 1);
            } else {
                RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_1);
                RC::GPIO::pwm_set_frequency(pcontext->pconfig->pconfigInfo->PWM_1, speed_v_m_s);
                RC::GPIO::digitalWrite(pcontext->pconfig->pconfigInfo->GPIO_1, RC_GPIO_LOW);
                RC::GPIO::pwm_start(pcontext->pconfig->pconfigInfo->PWM_1);
            }
        }

        void RobotCarMove::wheel_2_backward(float speed_v_m_s) {
            if (pcontext->pconfig->pconfigInfo->USE_MRAA) {
                convert_speed(speed_v_m_s, _pwm_2);
                set_value(_gpio_1, 0);
            } else {
                RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_2);
                RC::GPIO::pwm_set_frequency(pcontext->pconfig->pconfigInfo->PWM_2, speed_v_m_s);
                RC::GPIO::digitalWrite(pcontext->pconfig->pconfigInfo->GPIO_2, RC_GPIO_HIGH);
                RC::GPIO::pwm_start(pcontext->pconfig->pconfigInfo->PWM_2);
            }
        }

        void RobotCarMove::wheel_2_forward(float speed_v_m_s) {
            if (pcontext->pconfig->pconfigInfo->USE_MRAA) {
                convert_speed(speed_v_m_s, _pwm_2);
                set_value(_gpio_1, 1);
            } else {
                RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_2);
                RC::GPIO::pwm_set_frequency(pcontext->pconfig->pconfigInfo->PWM_2, speed_v_m_s);
                RC::GPIO::digitalWrite(pcontext->pconfig->pconfigInfo->GPIO_2, RC_GPIO_LOW);
                RC::GPIO::pwm_start(pcontext->pconfig->pconfigInfo->PWM_2);
            }
        }

        void RobotCarMove::wheel_3_backward(float speed_v_m_s) {
            if (pcontext->pconfig->pconfigInfo->USE_MRAA) {
                convert_speed(speed_v_m_s, _pwm_3);
                set_value(_gpio_1, 0);
            } else {
                RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_3);
                RC::GPIO::pwm_set_frequency(pcontext->pconfig->pconfigInfo->PWM_3, speed_v_m_s);
                RC::GPIO::digitalWrite(pcontext->pconfig->pconfigInfo->GPIO_3, RC_GPIO_HIGH);
                RC::GPIO::pwm_start(pcontext->pconfig->pconfigInfo->PWM_3);
            }
        }

        void RobotCarMove::wheel_3_forward(float speed_v_m_s) {
            if (pcontext->pconfig->pconfigInfo->USE_MRAA) {
                convert_speed(speed_v_m_s, _pwm_3);
                set_value(_gpio_1, 1);
            } else {
                RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_3);
                RC::GPIO::pwm_set_frequency(pcontext->pconfig->pconfigInfo->PWM_3, speed_v_m_s);
                RC::GPIO::digitalWrite(pcontext->pconfig->pconfigInfo->GPIO_3, RC_GPIO_LOW);
                RC::GPIO::pwm_start(pcontext->pconfig->pconfigInfo->PWM_3);
            }
        }

        void RobotCarMove::wheel_AC(float speed_1_v_m_s, float speed_2_v_m_s, float speed_3_v_m_s) {
            wheel_1_forward(speed_1_v_m_s);
            wheel_2_forward(speed_2_v_m_s);
            wheel_3_forward(speed_3_v_m_s);
        }

        void RobotCarMove::wheel_CW(float speed_1_v_m_s, float speed_2_v_m_s, float speed_3_v_m_s) {
            wheel_1_backward(speed_1_v_m_s);
            wheel_2_backward(speed_2_v_m_s);
            wheel_3_backward(speed_3_v_m_s);
        }

        void RobotCarMove::wheel_go_forward(float speed_1_v_m_s, float speed_2_v_m_s) {
            wheel_stop();
            wheel_1_backward(speed_1_v_m_s);
            wheel_2_forward(speed_2_v_m_s);
        }

        void RobotCarMove::wheel_go_backward(float speed_1_v_m_s, float speed_2_v_m_s) {
            wheel_stop();
            wheel_1_forward(speed_1_v_m_s);
            wheel_2_backward(speed_2_v_m_s);
        }

        void RobotCarMove::wheel_start() {
            if (pcontext->pconfig->pconfigInfo->USE_MRAA) {
                _pwm_1->enable(true);
                _pwm_2->enable(true);
                _pwm_3->enable(true);
            } else {
                RC::GPIO::pwm_start(pcontext->pconfig->pconfigInfo->PWM_1);
                RC::GPIO::pwm_start(pcontext->pconfig->pconfigInfo->PWM_2);
                RC::GPIO::pwm_start(pcontext->pconfig->pconfigInfo->PWM_3);
            }
        }

        void RobotCarMove::wheel_stop() {
            if (pcontext->pconfig->pconfigInfo->USE_MRAA) {
                _pwm_1->enable(false);
                _pwm_2->enable(false);
                _pwm_3->enable(false);
            } else {
                RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_1);
                RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_2);
                RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_3);
            }
        }

        void RobotCarMove::wheel_check(int times) {

        }

        void RobotCarMove::excute(WHEEL_DATA &wheelData) {
//        slog::info << __FILE__ << " " << __FUNCTION__ << " "
//                   << "wheel_1_v_m_s:" << wheelData.wheel_1_v_m_s
//                   << "wheel_2_v_m_s " << wheelData.wheel_2_v_m_s
//                   << "wheel_3_v_m_s " << wheelData.wheel_3_v_m_s
//                   << slog::endl;
            if (wheelData.wheel_1_v_m_s != 0) {
                if (wheelData.wheel_1_v_m_s < 0) {
                    this->wheel_1_forward(wheelData.wheel_1_v_m_s);
                } else
                    this->wheel_1_backward(wheelData.wheel_1_v_m_s);
            }
            if (wheelData.wheel_2_v_m_s != 0) {
                if (wheelData.wheel_2_v_m_s < 0) {
                    wheel_2_forward(wheelData.wheel_2_v_m_s);
                } else
                    wheel_2_backward(wheelData.wheel_2_v_m_s);
            }
            if (wheelData.wheel_3_v_m_s != 0) {
                if (wheelData.wheel_3_v_m_s < 0) {
                    wheel_3_forward(wheelData.wheel_2_v_m_s);
                } else
                    wheel_3_backward(wheelData.wheel_2_v_m_s);
            }
        }
    }
}

#endif //USE_DSP_DEVICE
